/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "vision-precomp.h"  // Precompiled headers
//
#include <mrpt/3rdparty/do_opencv_includes.h>
#include <mrpt/maps/CLandmarksMap.h>
#include <mrpt/math/geometry.h>
#include <mrpt/math/ops_matrices.h>
#include <mrpt/math/ops_vectors.h>
#include <mrpt/math/utils.h>
#include <mrpt/obs/CObservationBearingRange.h>
#include <mrpt/obs/CObservationStereoImages.h>
#include <mrpt/obs/CObservationVisualLandmarks.h>
#include <mrpt/poses/CPoint3D.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/vision/CFeature.h>
#include <mrpt/vision/CFeatureExtraction.h>
#include <mrpt/vision/pinhole.h>
#include <mrpt/vision/utils.h>

#include <Eigen/Dense>

using namespace mrpt;
using namespace mrpt::vision;
using namespace mrpt::img;
using namespace mrpt::config;
using namespace mrpt::maps;
using namespace mrpt::tfest;
using namespace mrpt::math;
using namespace mrpt::system;
using namespace mrpt::poses;
using namespace mrpt::obs;
using namespace std;
const int FEAT_FREE = -1;
// const int NOT_ASIG = 0;
// const int ASG_FEAT = 1;
// const int AMB_FEAT = 2;

/*-------------------------------------------------------------
          openCV_cross_correlation
-------------------------------------------------------------*/
void vision::openCV_cross_correlation(
    const CImage& img,
    const CImage& patch_img,
    size_t& x_max,
    size_t& y_max,
    double& max_val,
    int x_search_ini,
    int y_search_ini,
    int x_search_size,
    int y_search_size)
{
  MRPT_START

#if MRPT_HAS_OPENCV
  bool entireImg = (x_search_ini < 0 || y_search_ini < 0 || x_search_size < 0 || y_search_size < 0);

  CImage im, patch_im;

  if (img.isColor() && patch_img.isColor())
  {
    img.grayscale(im);
    patch_img.grayscale(patch_im);
  }
  else
  {
    ASSERT_(!img.isColor() && !patch_img.isColor());
    im = img.makeShallowCopy();
    patch_im = patch_img.makeShallowCopy();
  }

  const int im_w = im.getWidth();
  const int im_h = im.getHeight();
  const int patch_w = patch_im.getWidth();
  const int patch_h = patch_im.getHeight();

  if (entireImg)
  {
    x_search_size = im_w - patch_w;
    y_search_size = im_h - patch_h;
  }

  // JLBC: Perhaps is better to raise the exception always??
  if ((x_search_ini + x_search_size + patch_w) > im_w)
    x_search_size -= (x_search_ini + x_search_size + patch_w) - im_w;

  if ((y_search_ini + y_search_size + patch_h) > im_h)
    y_search_size -= (y_search_ini + y_search_size + patch_h) - im_h;

  ASSERT_((x_search_ini + x_search_size + patch_w) <= im_w);
  ASSERT_((y_search_ini + y_search_size + patch_h) <= im_h);
  CImage img_region_to_search;

  if (entireImg)
  {
    img_region_to_search = im.makeShallowCopy();
  }
  else
  {
    im.extract_patch(
        img_region_to_search,
        x_search_ini,  // start corner
        y_search_ini,
        patch_w + x_search_size,  // sub-image size
        patch_h + y_search_size);
  }

  cv::Mat result(cvSize(x_search_size + 1, y_search_size + 1), CV_32FC1);

  // Compute cross correlation:
  cv::matchTemplate(
      img_region_to_search.asCvMat<cv::Mat>(SHALLOW_COPY), patch_im.asCvMat<cv::Mat>(SHALLOW_COPY),
      result, cv::TM_CCORR_NORMED);

  // Find the max point:
  double mini;
  cv::Point min_point, max_point;

  cv::minMaxLoc(result, &mini, &max_val, &min_point, &max_point, cv::noArray());
  x_max = max_point.x + x_search_ini + (mrpt::round(patch_w - 1) >> 1);
  y_max = max_point.y + y_search_ini + (mrpt::round(patch_h - 1) >> 1);

#else
  THROW_EXCEPTION("MRPT has been compiled with MRPT_HAS_OPENCV=0 !");
#endif

  MRPT_END
}

/*-------------------------------------------------------------
          pixelTo3D
-------------------------------------------------------------*/
TPoint3D vision::pixelTo3D(const TPixelCoordf& xy, const CMatrixDouble33& A)
{
  TPoint3D res;

  // Build the vector:
  res.x = xy.x - A(0, 2);
  res.y = xy.y - A(1, 2);
  res.z = A(0, 0);

  // Normalize:
  const double u = res.norm();
  ASSERT_(u != 0);
  res *= 1.0 / u;

  return res;
}

/*-------------------------------------------------------------
          buildIntrinsicParamsMatrix
-------------------------------------------------------------*/
CMatrixDouble33 vision::buildIntrinsicParamsMatrix(
    const double focalLengthX,
    const double focalLengthY,
    const double centerX,
    const double centerY)
{
  CMatrixDouble33 A;

  A(0, 0) = focalLengthX;
  A(1, 1) = focalLengthY;
  A(2, 2) = 1;

  A(0, 2) = centerX;
  A(1, 2) = centerY;

  return A;
}

/*-------------------------------------------------------------
            computeMsd
-------------------------------------------------------------*/
double vision::computeMsd(const TMatchingPairList& feat_list, const poses::CPose3D& Rt)
{
  CMatrixDouble44 mat;
  Rt.getHomogeneousMatrix(mat);
  double acum = 0.0;

  TMatchingPairList::const_iterator it;
  TPoint3D err;
  for (it = feat_list.begin(); it != feat_list.end(); it++)
  {
    err.x = it->local.x - (it->global.x * mat(0, 0) + it->global.y * mat(0, 1) +
                           it->global.z * mat(0, 2) + Rt.x());
    err.y = it->local.y - (it->global.x * mat(1, 0) + it->global.y * mat(1, 1) +
                           it->global.z * mat(1, 2) + Rt.y());
    err.z = it->local.z - (it->global.x * mat(2, 0) + it->global.y * mat(2, 1) +
                           it->global.z * mat(2, 2) + Rt.z());

    acum += err.norm();

  }  // end for
  return (acum / feat_list.size());
}  // end msd

/*-------------------------------------------------------------
          cloudsToMatchedList
-------------------------------------------------------------*/
void vision::cloudsToMatchedList(
    const CObservationVisualLandmarks& cloud1,
    const CObservationVisualLandmarks& cloud2,
    TMatchingPairList& outList)
{
  CLandmarksMap::TCustomSequenceLandmarks::const_iterator itLand1, itLand2;
  TMatchingPair pair;

  for (itLand1 = cloud1.landmarks.landmarks.begin(); itLand1 != cloud1.landmarks.landmarks.end();
       itLand1++)
    for (itLand2 = cloud2.landmarks.landmarks.begin(); itLand2 != cloud2.landmarks.landmarks.end();
         itLand2++)
      if (itLand1->ID == itLand2->ID)
      {
        // Match found!
        pair.globalIdx = pair.localIdx = (unsigned int)itLand1->ID;

        pair.global.x = itLand1->pose_mean.x;
        pair.global.y = itLand1->pose_mean.y;
        pair.global.z = itLand1->pose_mean.z;

        pair.local.x = itLand2->pose_mean.x;
        pair.local.y = itLand2->pose_mean.y;
        pair.local.z = itLand2->pose_mean.z;

        outList.push_back(pair);
      }  // end if
}  // end-cloudsToMatchedList

/*-------------------------------------------------------------
          computeMainOrientation
-------------------------------------------------------------*/
float vision::computeMainOrientation(const CImage& image, unsigned int x, unsigned int y)
{
  MRPT_START
  float orientation = 0;
  if ((int(x) - 1 >= 0) && (int(y) - 1 >= 0) && (x + 1 < image.getWidth()) &&
      (y + 1 < image.getHeight()))
    orientation = (float)atan2(
        (double)*image(x, y + 1) - (double)*image(x, y - 1),
        (double)*image(x + 1, y) - (double)*image(x - 1, y));

  // Convert from [-pi,pi] to [0,2pi]

  return orientation;

  MRPT_END
}  // end vision::computeMainOrientation

/*-------------------------------------------------------------
          normalizeImage
-------------------------------------------------------------*/
void vision::normalizeImage(const CImage& image, CImage& nimage)
{
  ASSERT_(image.getChannelCount() == 1);
  nimage.resize(image.getWidth(), image.getHeight(), image.getChannelCount());

  CMatrixFloat im, nim;
  nim.resize(image.getHeight(), image.getWidth());

  image.getAsMatrix(im);

  double m, s;
  mrpt::math::meanAndStd(im, m, s);

  for (int k1 = 0; k1 < (int)nim.cols(); ++k1)
    for (int k2 = 0; k2 < (int)nim.rows(); ++k2) nim(k2, k1) = (im(k2, k1) - m) / s;

  nimage.setFromMatrix(nim);
}

/*-------------------------------------------------------------
            matchFeatures
-------------------------------------------------------------*/
size_t vision::matchFeatures(
    const CFeatureList& list1,
    const CFeatureList& list2,
    CMatchedFeatureList& matches,
    const TMatchingOptions& options,
    const TStereoSystemParams& params)
{
  // Clear the output structure
  MRPT_START
  // matches.clear();

  // Preliminary comprobations
  size_t sz1 = list1.size(), sz2 = list2.size();

  ASSERT_((sz1 > 0) && (sz2 > 0));                // Both lists have features within it
  ASSERT_(list1.get_type() == list2.get_type());  // Both lists must be of the same type

  CFeatureList::const_iterator itList1, itList2;  // Iterators for the lists

  // For SIFT & SURF
  float distDesc;  // EDD or EDSD
  float minDist1;  // Minimum EDD or EDSD
  float minDist2;  // Second minimum EDD or EDSD

  // For Harris
  double maxCC1;  // Maximum CC
  double maxCC2;  // Second maximum CC

  // For SAD
  double minSAD1, minSAD2;

  vector<int> idxLeftList, idxRightList;
  idxLeftList.resize(sz1, FEAT_FREE);
  idxRightList.resize(sz2, FEAT_FREE);
  vector<double> distCorrs(sz1);
  int lFeat, rFeat;
  int minLeftIdx = 0, minRightIdx;
  int nMatches = 0;

  // For each feature in list1 ...
  for (lFeat = 0, itList1 = list1.begin(); itList1 != list1.end(); ++itList1, ++lFeat)
  {
    // For SIFT & SURF
    minDist1 = 1e5;
    minDist2 = 1e5;

    // For Harris
    maxCC1 = 0;
    maxCC2 = 0;

    // For SAD
    minSAD1 = 1e5;
    minSAD2 = 1e5;

    // For all the cases
    minRightIdx = 0;

    for (rFeat = 0, itList2 = list2.begin(); itList2 != list2.end();
         ++itList2, ++rFeat)  // ... compare with all the features in list2.
    {
      // Filter out by epipolar constraint
      double d = 0.0;  // Distance to the epipolar line
      if (options.useEpipolarRestriction)
      {
        if (options.parallelOpticalAxis)
          d = itList1->keypoint.pt.y - itList2->keypoint.pt.y;
        else
        {
          ASSERT_(options.hasFundamentalMatrix);

          // Compute epipolar line Ax + By + C = 0
          TLine2D epiLine;
          TPoint2D oPoint(itList2->keypoint.pt.x, itList2->keypoint.pt.y);

          CMatrixDouble31 l, p;
          p(0, 0) = itList1->keypoint.pt.x;
          p(1, 0) = itList1->keypoint.pt.y;
          p(2, 0) = 1;

          l = params.F * p;

          epiLine.coefs[0] = l(0, 0);
          epiLine.coefs[1] = l(1, 0);
          epiLine.coefs[2] = l(2, 0);

          d = epiLine.distance(oPoint);
        }  // end else
      }    // end if

      // Use epipolar restriction
      bool c1 = options.useEpipolarRestriction ? fabs(d) < options.epipolar_TH : true;
      // Use x-coord restriction
      bool c2 =
          options.useXRestriction ? itList1->keypoint.pt.x - itList2->keypoint.pt.x > 0 : true;

      if (c1 && c2)
      {
        switch (options.matching_method)
        {
          case TMatchingOptions::mmDescriptorSIFT:
          {
            // Ensure that both features have SIFT descriptors
            ASSERT_(
                itList1->descriptors.hasDescriptorSIFT() &&
                itList2->descriptors.hasDescriptorSIFT());

            // Compute the Euclidean distance between descriptors
            distDesc = itList1->descriptorSIFTDistanceTo(*itList2);

            // Search for the two minimum values
            if (distDesc < minDist1)
            {
              minDist2 = minDist1;
              minDist1 = distDesc;
              minLeftIdx = lFeat;
              minRightIdx = rFeat;
            }
            else if (distDesc < minDist2)
              minDist2 = distDesc;

            break;
          }  // end mmDescriptorSIFT

          case TMatchingOptions::mmCorrelation:
          {
            size_t u, v;  // Coordinates of the peak
            double res;   // Value of the peak

            // Ensure that both features have patches
            ASSERT_(itList1->patchSize > 0 && itList2->patchSize > 0);
            vision::openCV_cross_correlation(*itList1->patch, *itList2->patch, u, v, res);

            // Search for the two maximum values
            if (res > maxCC1)
            {
              maxCC2 = maxCC1;
              maxCC1 = res;
              minLeftIdx = lFeat;
              minRightIdx = rFeat;
            }
            else if (res > maxCC2)
              maxCC2 = res;

            break;
          }  // end mmCorrelation

          case TMatchingOptions::mmDescriptorSURF:
          {
            // Ensure that both features have SURF descriptors
            ASSERT_(
                itList1->descriptors.hasDescriptorSURF() &&
                itList2->descriptors.hasDescriptorSURF());

            // Compute the Euclidean distance between descriptors
            distDesc = itList1->descriptorSURFDistanceTo(*itList2);

            // Search for the two minimum values
            if (distDesc < minDist1)
            {
              minDist2 = minDist1;
              minDist1 = distDesc;
              minLeftIdx = lFeat;
              minRightIdx = rFeat;
            }
            else if (distDesc < minDist2)
              minDist2 = distDesc;

            break;  // end case featSURF
          }         // end mmDescriptorSURF

          case TMatchingOptions::mmDescriptorORB:
          {
            // Ensure that both features have SURF descriptors
            ASSERT_(
                itList1->descriptors.hasDescriptorORB() && itList2->descriptors.hasDescriptorORB());
            distDesc = itList1->descriptorORBDistanceTo(*itList2);

            // Search for the two minimum values
            if (distDesc < minDist1)
            {
              minDist2 = minDist1;
              minDist1 = distDesc;
              minLeftIdx = lFeat;
              minRightIdx = rFeat;
            }
            else if (distDesc < minDist2)
              minDist2 = distDesc;

            break;
          }  // end mmDescriptorORB

          case TMatchingOptions::mmSAD:
          {
            // Ensure that both features have patches
            ASSERT_(itList1->patchSize > 0 && itList2->patchSize == itList1->patchSize);
#if !MRPT_HAS_OPENCV
            THROW_EXCEPTION("MRPT has been compiled without OpenCV");
#else
            const CImage aux1(*itList1->patch, FAST_REF_OR_CONVERT_TO_GRAY);
            const CImage aux2(*itList2->patch, FAST_REF_OR_CONVERT_TO_GRAY);
            const auto h = aux1.getHeight(), w = aux1.getWidth();

            double res = 0;
            for (unsigned int ii = 0; ii < h; ++ii)
              for (unsigned int jj = 0; jj < w; ++jj)
                res += std::abs(
                    static_cast<double>(aux1.at<uint8_t>(jj, ii)) -
                    static_cast<double>(aux2.at<uint8_t>(jj, ii)));
            res = res / (255.0 * w * h);

            if (res < minSAD1)
            {
              minSAD2 = minSAD1;
              minSAD1 = res;
              minLeftIdx = lFeat;
              minRightIdx = rFeat;
            }
            else if (res < minSAD2)
              minSAD2 = res;
#endif
            break;
          }  // end mmSAD
        }    // end switch
      }      // end if
    }        // end for 'list2' (right features)

    bool cond1 = false, cond2 = false;
    double minVal = 1.0;
    switch (options.matching_method)
    {
      case TMatchingOptions::mmDescriptorSIFT:
        cond1 = minDist1 < options.maxEDD_TH;  // Maximum Euclidean
        // Distance between SIFT
        // descriptors (EDD)
        cond2 = (minDist1 / minDist2) < options.EDD_RATIO;  // Ratio between the two lowest EDSD
        minVal = minDist1;
        break;
      case TMatchingOptions::mmCorrelation:
        cond1 = maxCC1 > options.minCC_TH;           // Minimum cross correlation value
        cond2 = (maxCC2 / maxCC1) < options.rCC_TH;  // Ratio between the two highest cross
        // correlation values
        minVal = 1 - maxCC1;
        break;
      case TMatchingOptions::mmDescriptorSURF:
        cond1 = minDist1 < options.maxEDSD_TH;  // Maximum Euclidean
        // Distance between SURF
        // descriptors (EDSD)
        cond2 = (minDist1 / minDist2) < options.EDSD_RATIO;  // Ratio between the two lowest EDSD
        minVal = minDist1;
        break;
      case TMatchingOptions::mmSAD:
        cond1 = minSAD1 < options.maxSAD_TH;
        cond2 = (minSAD1 / minSAD2) < options.SAD_RATIO;
        minVal = minSAD1;
        break;
      case TMatchingOptions::mmDescriptorORB:
        cond1 = minDist1 < options.maxORB_dist;
        cond2 = true;
        minVal = minDist1;
        break;
      default:
        THROW_EXCEPTION("Invalid value of 'matching_method'");
    }

    // PROCESS THE RESULTS
    if (cond1 && cond2)  // The minimum distance must be below a threshold
    {
      int auxIdx = idxRightList[minRightIdx];
      if (auxIdx != FEAT_FREE)
      {
        if (distCorrs[auxIdx] > minVal)
        {
          // We've found a better match
          distCorrs[minLeftIdx] = minVal;
          idxLeftList[minLeftIdx] = minRightIdx;
          idxRightList[minRightIdx] = minLeftIdx;

          distCorrs[auxIdx] = 1.0;
          idxLeftList[auxIdx] = FEAT_FREE;
        }  // end-if
      }    // end-if
      else
      {
        idxRightList[minRightIdx] = minLeftIdx;
        idxLeftList[minLeftIdx] = minRightIdx;
        distCorrs[minLeftIdx] = minVal;
        nMatches++;
      }
    }  // end if
  }    // end for 'list1' (left features)

  if (!options.addMatches) matches.clear();

  TFeatureID idLeft = 0, idRight = 0;
  if (!matches.empty()) matches.getMaxID(bothLists, idLeft, idRight);

  for (int vCnt = 0; vCnt < (int)idxLeftList.size(); ++vCnt)
  {
    if (idxLeftList[vCnt] != FEAT_FREE)
    {
      std::pair<CFeature, CFeature> thisMatch;

      bool isGood = true;
      double dp1 = -1.0, dp2 = -1.0;
      TPoint3D p3D = TPoint3D();
      if (options.estimateDepth && options.parallelOpticalAxis)
      {
        projectMatchedFeature(list1[vCnt], list2[idxLeftList[vCnt]], p3D, params);
        dp1 = sqrt(p3D.x * p3D.x + p3D.y * p3D.y + p3D.z * p3D.z);
        dp2 = sqrt(
            (p3D.x - params.baseline) * (p3D.x - params.baseline) + p3D.y * p3D.y + p3D.z * p3D.z);

        if (dp1 > options.maxDepthThreshold || dp2 > options.maxDepthThreshold) isGood = false;
      }  // end-if

      if (isGood)
      {
        // Set the features
        thisMatch.first = list1[vCnt];
        thisMatch.second = list2[idxLeftList[vCnt]];

        // Update the max ID value
        if (matches.empty())
        {
          idLeft = thisMatch.first.keypoint.ID;
          idRight = thisMatch.second.keypoint.ID;
        }
        else
        {
          keep_max(idLeft, thisMatch.first.keypoint.ID);
          matches.setLeftMaxID(idLeft);

          keep_max(idRight, thisMatch.second.keypoint.ID);
          matches.setRightMaxID(idRight);
        }

        // Set the depth and the 3D position of the feature
        if (options.estimateDepth && options.parallelOpticalAxis)
        {
          thisMatch.first.initialDepth = dp1;
          thisMatch.first.p3D = p3D;

          thisMatch.second.initialDepth = dp2;
          thisMatch.second.p3D = TPoint3D(p3D.x - params.baseline, p3D.y, p3D.z);
        }  // end-if

        // Insert the match into the matched list
        matches.push_back(thisMatch);
      }  // end-if-isGood
    }    // end-if
  }      // end-for-matches
  return matches.size();

  MRPT_END
}

/*-------------------------------------------------------------
      generateMask
-------------------------------------------------------------*/
// Insert zeros around the points in mList according to wSize
void vision::generateMask(
    const CMatchedFeatureList& mList, CMatrixBool& mask1, CMatrixBool& mask2, int wSize)
{
  ASSERT_(mList.size() > 0);

  //    cv::Mat *mask1 = static_cast<cv::Mat*>(_mask1);
  //    cv::Mat *mask2 = static_cast<cv::Mat*>(_mask2);

  int hwsize = (int)(0.5 * wSize);
  int mx = mask1.cols(), my = mask1.rows();

  int idx, idy;
  CMatchedFeatureList::const_iterator it;
  for (it = mList.begin(); it != mList.end(); ++it)
  {
    for (int ii = -hwsize; ii < hwsize; ++ii)
      for (int jj = -hwsize; jj < hwsize; ++jj)
      {
        idx = (int)(it->first.keypoint.pt.x) + ii;
        idy = (int)(it->first.keypoint.pt.y) + jj;
        if (idx >= 0 && idy >= 0 && idx < mx && idy < my) mask1(idy, idx) = false;
      }

    for (int ii = -hwsize; ii < hwsize; ++ii)
      for (int jj = -hwsize; jj < hwsize; ++jj)
      {
        idx = (int)(it->second.keypoint.pt.x) + ii;
        idy = (int)(it->second.keypoint.pt.y) + jj;
        if (idx >= 0 && idy >= 0 && idx < mx && idy < my) mask2(idy, idx) = false;
      }
  }  // end-for
}  // end generateMask

double vision::computeSAD(const CImage& p1, const CImage& p2)
{
  MRPT_START
#if MRPT_HAS_OPENCV
  ASSERT_(p1.getSize() == p2.getSize());
  const auto w = p1.getWidth(), h = p1.getHeight();
  double res = 0.0;
  for (unsigned int ii = 0; ii < h; ++ii)
    for (unsigned int jj = 0; jj < w; ++jj)
      res += std::abs(
          static_cast<double>(p1.at<uint8_t>(jj, ii)) -
          static_cast<double>(p2.at<uint8_t>(jj, ii)));

  return res / (255.0 * w * h);
#else
  THROW_EXCEPTION("MRPT compiled without OpenCV, can't compute SAD of images!");
#endif
  MRPT_END
}

void vision::addFeaturesToImage(const CImage& inImg, const CFeatureList& theList, CImage& outImg)
{
  outImg = inImg;  // Create a copy of the input image
  for (const auto& it : theList)
    outImg.rectangle(
        it.keypoint.pt.x - 5, it.keypoint.pt.y - 5, it.keypoint.pt.x + 5, it.keypoint.pt.y + 5,
        TColor(255, 0, 0));
}

/*-------------------------------------------------------------
          projectMatchedFeatures
-------------------------------------------------------------*/
void vision::projectMatchedFeatures(
    const CMatchedFeatureList& matches,
    const mrpt::img::TStereoCamera& stereo_camera,
    vector<TPoint3D>& out_points)
{
  out_points.clear();
  out_points.reserve(matches.size());
  for (const auto& match : matches)
  {
    const auto& pt1 = match.first.keypoint.pt;
    const auto& pt2 = match.second.keypoint.pt;

    const double disp = pt1.x - pt2.x;
    if (disp < 1) continue;

    const double b_d = stereo_camera.rightCameraPose.x / disp;
    out_points.emplace_back(
        (pt1.x - stereo_camera.leftCamera.cx()) * b_d,
        (pt1.y - stereo_camera.leftCamera.cy()) * b_d, stereo_camera.leftCamera.fx() * b_d);
  }
}
/*-------------------------------------------------------------
          projectMatchedFeatures
-------------------------------------------------------------*/
void vision::projectMatchedFeatures(
    const CFeatureList& leftList,
    const CFeatureList& rightList,
    vector<TPoint3D>& vP3D,
    const TStereoSystemParams& params)
{
  vP3D.reserve(leftList.size());
  CFeatureList::const_iterator it1, it2;
  for (it1 = leftList.begin(), it2 = rightList.begin(); it1 != leftList.end(); ++it1, ++it2)
  {
    TPoint3D p3D;
    projectMatchedFeature(*it1, *it2, p3D, params);
    if (p3D.z < params.maxZ && p3D.z > params.minZ && p3D.y < params.maxY) vP3D.push_back(p3D);
  }
}  // end projectMatchedFeatures

/*-------------------------------------------------------------
          projectMatchedFeatures
-------------------------------------------------------------*/
void vision::projectMatchedFeature(
    const CFeature& leftFeat,
    const CFeature& rightFeat,
    TPoint3D& p3D,
    const TStereoSystemParams& params)
{
  const double f0 = 600;
  double nfx1 = leftFeat.keypoint.pt.x, nfy1 = leftFeat.keypoint.pt.y,
         nfx2 = rightFeat.keypoint.pt.x, nfy2 = rightFeat.keypoint.pt.y;

  const double x = nfx1 * f0;   // x  = (x  / f0) * f0   x  = x
  const double y = nfy1 * f0;   // y  = (y  / f0) * f0   y  = y
  const double xd = nfx2 * f0;  // x' = (x' / f0) * f0   x' = x'
  const double yd = nfy2 * f0;  // y' = (y' / f0) * f0   y' = y'

  const double f2 = f0 * f0;
  const double p9 = f2 * params.F(2, 2);
  const double Q00 = f2 * (params.F(0, 2) * params.F(0, 2) + params.F(1, 2) * params.F(1, 2) +
                           params.F(2, 0) * params.F(2, 0) + params.F(2, 1) * params.F(2, 1));

  double Jh = (std::numeric_limits<double>::max)();  // J hat = 
  double xh = x;                                     // x hat = x
  double yh = y;                                     // y hat = y
  double xhd = xd;                                   // x hat dash = x'
  double yhd = yd;                                   // y hat dash = y'
  double xt = 0, yt = 0, xtd = 0,
         ytd = 0;  // x tilde = 0, y tilde = 0, x tilde dash = 0, y tilde dash = 0
  for (;;)
  {
    const double p1 = (xh * xhd + xhd * xt + xh * xtd) * params.F(0, 0);
    const double p2 = (xh * yhd + yhd * xt + xh * ytd) * params.F(0, 1);
    const double p3 = (f0 * (xh + xt)) * params.F(0, 2);
    const double p4 = (yh * xhd + xhd * yt + yh * xtd) * params.F(1, 0);
    const double p5 = (yh * yhd + yhd * yt + yh * ytd) * params.F(1, 1);
    const double p6 = (f0 * (yh + yt)) * params.F(1, 2);
    const double p7 = (f0 * (xhd + xtd)) * params.F(2, 0);
    const double p8 = (f0 * (yhd + ytd)) * params.F(2, 1);

    const double udotxi = p1 + p2 + p3 + p4 + p5 + p6 + p7 + p8 + p9;

    const double Q11 = (xh * xh + xhd * xhd) * params.F(0, 0) * params.F(0, 0);
    const double Q22 = (xh * xh + yhd * yhd) * params.F(0, 1) * params.F(0, 1);
    const double Q44 = (yh * yh + xhd * xhd) * params.F(1, 0) * params.F(1, 0);
    const double Q55 = (yh * yh + yhd * yhd) * params.F(1, 1) * params.F(1, 1);
    const double Q12 = xhd * yhd * params.F(0, 0) * params.F(0, 1);
    const double Q13 = f0 * xhd * params.F(0, 0) * params.F(0, 2);
    const double Q14 = xh * yh * params.F(0, 0) * params.F(1, 0);
    const double Q17 = f0 * xh * params.F(0, 0) * params.F(2, 0);
    const double Q23 = f0 * yhd * params.F(0, 1) * params.F(0, 2);
    const double Q25 = xh * yh * params.F(0, 1) * params.F(1, 1);
    const double Q28 = f0 * xh * params.F(0, 1) * params.F(2, 1);
    const double Q45 = xhd * yhd * params.F(1, 0) * params.F(1, 1);
    const double Q46 = f0 * xhd * params.F(1, 0) * params.F(1, 2);
    const double Q47 = f0 * yh * params.F(1, 0) * params.F(2, 0);
    const double Q56 = f0 * yhd * params.F(1, 1) * params.F(1, 2);
    const double Q58 = f0 * yh * params.F(1, 1) * params.F(2, 1);

    const double udotV0xiu =
        Q00 + Q11 + Q22 + Q44 + Q55 +
        2.0 * (Q12 + Q13 + Q14 + Q17 + Q23 + Q25 + Q28 + Q45 + Q46 + Q47 + Q56 + Q58);

    ASSERT_(fabs(udotV0xiu) > 1e-5);

    const double C = udotxi / udotV0xiu;

    xt = C * (params.F(0, 0) * xhd + params.F(0, 1) * yhd + f0 * params.F(0, 2));
    yt = C * (params.F(1, 0) * xhd + params.F(1, 1) * yhd + f0 * params.F(1, 2));
    xtd = C * (params.F(0, 0) * xh + params.F(1, 0) * yh + f0 * params.F(2, 0));
    ytd = C * (params.F(0, 1) * xh + params.F(1, 1) * yh + f0 * params.F(2, 1));

    const double Jt = xt * xt + yt * yt + xtd * xtd + ytd * ytd;
    //        cout << "Jt:" << Jt << " and Jh: " << Jh << endl;
    if ((std::abs)(Jt - Jh) <= 1e-5)
    {
      nfx1 = xh / f0;
      nfy1 = yh / f0;
      nfx2 = xhd / f0;
      // nfy2 = yhd / f0;

      break;
    }
    else
    {
      Jh = Jt;         // J hat      = J tilde
      xh = x - xt;     // x hat      = x  - x tilde
      yh = y - yt;     // y hat      = y  - y tilde
      xhd = xd - xtd;  // x hat dash = x' - x tilde dash
      yhd = yd - ytd;  // y hat dash = y' - y tilde dash
    }
  }  // end for

  double disp = nfx1 - nfx2;
  double aux = params.baseline / disp;
  p3D.x = (nfx1 - params.K(0, 2)) * aux;
  p3D.y = (nfy1 - params.K(1, 2)) * aux;
  p3D.z = params.K(0, 0) * aux;
}  // end projectMatchedFeature

/*-------------------------------------------------------------
          projectMatchedFeatures
-------------------------------------------------------------*/
void vision::projectMatchedFeatures(
    CMatchedFeatureList& mfList, const TStereoSystemParams& param, CLandmarksMap& landmarks)
{
  MRPT_START

  landmarks.clear();  // Assert that the output CLandmarksMap is clear

  float stdPixel2 = square(param.stdPixel);
  float stdDisp2 = square(param.stdDisp);

  // Main loop
  for (auto itList = mfList.begin(); itList != mfList.end();)
  {
    const auto& pt1 = itList->first.keypoint.pt;
    const auto& pt2 = itList->second.keypoint.pt;

    const float disp = pt1.x - pt2.x;  // Disparity
    if (disp < 1e-9)
    {
      // Filter out too far points
      // Erase the match
      itList = mfList.erase(itList);
      continue;
    }

    float x3D = (pt1.x - param.K(0, 2)) * ((param.baseline)) / disp;
    float y3D = (pt1.y - param.K(1, 2)) * ((param.baseline)) / disp;
    float z3D = (param.K(0, 0)) * ((param.baseline)) / disp;

    // Filter out bad points
    if ((z3D < param.minZ) || (z3D > param.maxZ))
    {
      itList = mfList.erase(itList);
    }
    else
    {
      TPoint3D p3D(x3D, y3D, z3D);

      // STORE THE OBTAINED LANDMARK
      CLandmark lm;

      TPoint3D norm3D = p3D;
      norm3D *= -1 / norm3D.norm();

      lm.normal = norm3D;
      lm.pose_mean = p3D;
      lm.ID = itList->first.keypoint.ID;

      // If the matched landmarks has a (SIFT or SURF) descriptor,
      // asign the left one to the landmark.
      // TO DO: Assign the mean value of the descriptor (between the
      // matches)
      lm.features.resize(1);
      lm.features[0] = (*itList).first;

      // Compute the covariance matrix for the landmark
      switch (param.uncPropagation)
      {
        case TStereoSystemParams::Prop_Linear:
        {
          float foc2 = square(param.K(0, 0));
          float c0 = param.K(0, 2);
          float r0 = param.K(1, 2);
          float base2 = square(param.baseline);
          float disp2 = square(disp);

          lm.pose_cov_11 =
              stdPixel2 * base2 / disp2 + stdDisp2 * base2 * square(pt1.x - c0) / square(disp2);
          lm.pose_cov_12 = stdDisp2 * base2 * (pt1.x - c0) * (pt1.y - r0) / square(disp2);
          lm.pose_cov_13 = stdDisp2 * base2 * sqrt(foc2) * (pt1.x - c0) / square(disp2);
          lm.pose_cov_22 =
              stdPixel2 * base2 / disp2 + stdDisp2 * base2 * square(pt1.y - r0) / square(disp2);
          lm.pose_cov_23 = stdDisp2 * base2 * sqrt(foc2) * (pt1.y - r0) / square(disp2);
          lm.pose_cov_33 = stdDisp2 * foc2 * base2 / square(disp2);
        }  // end case 'Prop_Linear'
        break;

        case TStereoSystemParams::Prop_UT:
        {
          // Parameters
          unsigned int Na = 3;
          unsigned int i;

          float k = param.factor_k;

          float w0 = k / (Na + k);
          float w1 = 1 / (2 * (Na + k));

          CMatrixF Pa(3, 3);
          CMatrixF L(3, 3);

          Pa.fill(0);
          Pa(0, 0) = Pa(1, 1) = (Na + k) * square(param.stdPixel);
          Pa(2, 2) = (Na + k) * square(param.stdDisp);

          // Cholesky decomposition
          Pa.chol(L);  // math::chol(Pa,L);

          vector<TPoint3D> B;  // B group
          TPoint3D meanB;      // Mean value of the B group
          CMatrixF Pb;         // Covariance of the B group

          B.resize(2 * Na + 1);  // Set of output values
          Pb.fill(0);            // Reset the output covariance

          CVectorFloat vAux, myPoint;  // Auxiliar vectors
          CVectorFloat meanA;          // Mean value of the A group

          vAux.resize(3);  // Set the variables size
          meanA.resize(3);
          myPoint.resize(3);

          // Mean input value: (c,r,d)
          meanA[0] = pt1.x;
          meanA[1] = pt1.y;
          meanA[2] = disp;

          // Output mean
          meanB.x = w0 * x3D;
          meanB.y = w0 * y3D;
          meanB.z = w0 * z3D;  // Add to the mean
          B[0].x = x3D;
          B[0].y = y3D;
          B[0].z = z3D;  // Insert into B

          for (i = 1; i <= 2 * Na; i++)
          {
            // Form the Ai value
            if (i <= Na)
            {
              // Extract the proper row
              vAux.asEigen() = L.asEigen().row(i - 1);
              myPoint[0] = meanA[0] + vAux[0];
              myPoint[1] = meanA[1] + vAux[1];
              myPoint[2] = meanA[2] + vAux[2];
            }
            else
            {
              vAux.asEigen() = L.asEigen().row((i - Na) - 1);
              myPoint[0] = meanA[0] - vAux[0];
              myPoint[1] = meanA[1] - vAux[1];
              myPoint[2] = meanA[2] - vAux[2];
            }

            // Pass the Ai through the functions:
            x3D = (myPoint[0] - param.K(0, 2)) * ((param.baseline)) / myPoint[2];
            y3D = (myPoint[1] - param.K(1, 2)) * ((param.baseline)) / myPoint[2];
            z3D = (param.K(0, 0)) * ((param.baseline)) / myPoint[2];

            // Add to the B mean computation and the B vector
            meanB.x = meanB.x + w1 * x3D;
            meanB.y = meanB.y + w1 * y3D;
            meanB.z = meanB.z + w1 * z3D;

            B[i].x = x3D;
            B[i].y = y3D;
            B[i].z = z3D;

          }  // end for 'i'

          // Output covariance
          for (i = 0; i <= 2 * Na; i++)
          {
            float weight = w1;
            CMatrixF v(3, 1);

            if (i == 0)  // The weight for the mean value of A is w0
              weight = w0;

            v(0, 0) = B[i].x - meanB.x;
            v(1, 0) = B[i].y - meanB.y;
            v(2, 0) = B[i].z - meanB.z;

            Pb.asEigen() += (weight * (v.asEigen() * v.transpose())).eval();
          }  // end for 'i'

          // Store it in the landmark
          lm.pose_cov_11 = Pb(0, 0);
          lm.pose_cov_12 = Pb(0, 1);
          lm.pose_cov_13 = Pb(0, 2);
          lm.pose_cov_22 = Pb(1, 1);
          lm.pose_cov_23 = Pb(1, 2);
          lm.pose_cov_33 = Pb(2, 2);
        }  // end case 'Prop_UT'
        break;

        case TStereoSystemParams::Prop_SUT:
        {
          // Parameters
          unsigned int Na = 3;
          unsigned int i;

          float a = param.factor_a;
          float b = param.factor_b;
          float k = param.factor_k;

          float lambda = square(a) * (Na + k) - Na;

          float w0_m = lambda / (Na + lambda);
          float w0_c = w0_m + (1 - square(a) + b);
          float w1 = 1 / (2 * (Na + lambda));

          CMatrixF Pa(3, 3);
          CMatrixF L(3, 3);

          Pa.fill(0);
          Pa(0, 0) = Pa(1, 1) = (Na + lambda) * square(param.stdPixel);
          Pa(2, 2) = (Na + lambda) * square(param.stdDisp);

          // Cholesky decomposition
          Pa.chol(L);  // math::chol(Pa,L);

          vector<TPoint3D> B;  // B group
          TPoint3D meanB;      // Mean value of the B group
          CMatrixF Pb;         // Covariance of the B group

          B.resize(2 * Na + 1);  // Set of output values
          Pb.fill(0);            // Reset the output covariance

          CVectorFloat vAux, myPoint;  // Auxiliar vectors
          CVectorFloat meanA;          // Mean value of the A group

          vAux.resize(3);  // Set the variables size
          meanA.resize(3);
          myPoint.resize(3);

          // Mean input value: (c,r,d)
          meanA[0] = pt1.x;
          meanA[1] = pt1.y;
          meanA[2] = disp;

          // Output mean
          meanB.x = w0_m * x3D;
          meanB.y = w0_m * y3D;
          meanB.z = w0_m * z3D;  // Add to the mean
          B[0].x = x3D;
          B[0].y = y3D;
          B[0].z = z3D;  // Insert into B

          for (i = 1; i <= 2 * Na; i++)
          {
            // Form the Ai value
            if (i <= Na)
            {
              // Extract the proper row
              vAux.asEigen() = L.row(i - 1);
              myPoint = meanA + vAux;
              // myPoint[0] = meanA[0] + vAux[0];
              // myPoint[1] = meanA[1] + vAux[1];
              // myPoint[2] = meanA[2] + vAux[2];
            }
            else
            {
              vAux = L.row((i - Na) - 1);
              myPoint = meanA - vAux;
              // myPoint[0] = meanA[0] - vAux[0];
              // myPoint[1] = meanA[1] - vAux[1];
              // myPoint[2] = meanA[2] - vAux[2];
            }

            // Pass the Ai through the functions:
            x3D = (myPoint[0] - param.K(0, 2)) * ((param.baseline)) / myPoint[2];
            y3D = (myPoint[1] - param.K(1, 2)) * ((param.baseline)) / myPoint[2];
            z3D = (param.K(0, 0)) * ((param.baseline)) / myPoint[2];

            // Add to the B mean computation and the B vector
            meanB.x = meanB.x + w1 * x3D;
            meanB.y = meanB.y + w1 * y3D;
            meanB.z = meanB.z + w1 * z3D;

            B[i].x = x3D;
            B[i].y = y3D;
            B[i].z = z3D;

          }  // end for 'i'

          // Output covariance
          for (i = 0; i <= 2 * Na; i++)
          {
            float weight = w1;
            CMatrixF v(3, 1);

            if (i == 0)  // The weight for the mean value of A is w0
              weight = w0_c;

            v(0, 0) = B[i].x - meanB.x;
            v(1, 0) = B[i].y - meanB.y;
            v(2, 0) = B[i].z - meanB.z;

            Pb.asEigen() += (weight * (v.asEigen() * v.transpose())).eval();
          }  // end for 'i'

          // Store it in the landmark
          lm.pose_cov_11 = Pb(0, 0);
          lm.pose_cov_12 = Pb(0, 1);
          lm.pose_cov_13 = Pb(0, 2);
          lm.pose_cov_22 = Pb(1, 1);
          lm.pose_cov_23 = Pb(1, 2);
          lm.pose_cov_33 = Pb(2, 2);
        }  // end case 'Prop_SUT'
        break;

      }  // end switch
      landmarks.landmarks.push_back(lm);
      itList++;
    }  // end else ( (z3D > param.minZ) && (z3D < param.maxZ) )
  }    // end for 'i'

  MRPT_END
}  // end of projectMatchedFeatures

/*-------------------------------------------------------------
          projectMatchedFeatures
-------------------------------------------------------------*/
void vision::projectMatchedFeatures(
    CFeatureList& leftList,
    CFeatureList& rightList,
    const vision::TStereoSystemParams& param,
    mrpt::maps::CLandmarksMap& landmarks)
{
  MRPT_START
  ASSERT_(leftList.size() == rightList.size());

  landmarks.clear();  // Assert that the output CLandmarksMap is clear

  CFeatureList::iterator itListL, itListR;
  float stdPixel2 = square(param.stdPixel);
  float stdDisp2 = square(param.stdDisp);

  // Main loop
  for (itListL = leftList.begin(), itListR = rightList.begin(); itListL != leftList.end();)
  {
    const auto& ptL = itListL->keypoint.pt;
    const auto& ptR = itListR->keypoint.pt;

    float disp = ptL.x - ptR.x;  // Disparity
    if (disp < 1e-9)             // Filter out too far points
    {
      itListL = leftList.erase(itListL);
      itListR = rightList.erase(itListR);
      continue;
    }

    // Too much distant features are not taken into account
    float x3D = (ptL.x - param.K(0, 2)) * param.baseline / disp;
    float y3D = (ptL.y - param.K(1, 2)) * param.baseline / disp;
    float z3D = (param.K(0, 0)) * param.baseline / disp;

    // Filter out bad points
    if ((z3D < param.minZ) || (z3D > param.maxZ))
    {
      itListL = leftList.erase(itListL);
      itListR = rightList.erase(itListR);
    }
    else
    {
      TPoint3D p3D(x3D, y3D, z3D);

      // STORE THE OBTAINED LANDMARK
      CLandmark lm;

      TPoint3D norm3D = p3D;
      norm3D *= -1. / norm3D.norm();

      lm.normal = norm3D;
      lm.pose_mean = p3D;
      lm.ID = itListL->keypoint.ID;

      // If the matched landmarks has a (SIFT or SURF) descriptor,
      // asign the left one to the landmark.
      // TO DO: Assign the mean value of the descriptor (between the
      // matches)
      lm.features.resize(2);
      lm.features[0] = *itListL;
      lm.features[1] = *itListR;

      // Compute the covariance matrix for the landmark
      switch (param.uncPropagation)
      {
        case TStereoSystemParams::Prop_Linear:
        {
          float foc2 = square(param.K(0, 0));
          float c0 = param.K(0, 2);
          float r0 = param.K(1, 2);
          float base2 = square(param.baseline);
          float disp2 = square(ptL.x - ptR.x);

          lm.pose_cov_11 =
              stdPixel2 * base2 / disp2 + stdDisp2 * base2 * square(ptL.x - c0) / square(disp2);
          lm.pose_cov_12 = stdDisp2 * base2 * (ptL.x - c0) * (ptL.y - r0) / square(disp2);
          lm.pose_cov_13 = stdDisp2 * base2 * sqrt(foc2) * (ptL.x - c0) / square(disp2);
          lm.pose_cov_22 =
              stdPixel2 * base2 / disp2 + stdDisp2 * base2 * square(ptL.y - r0) / square(disp2);
          lm.pose_cov_23 = stdDisp2 * base2 * sqrt(foc2) * (ptL.y - r0) / square(disp2);
          lm.pose_cov_33 = stdDisp2 * foc2 * base2 / square(disp2);
        }  // end case 'Prop_Linear'
        break;

        case TStereoSystemParams::Prop_UT:
        {
          // Parameters
          unsigned int Na = 3;
          unsigned int i;

          float k = param.factor_k;

          float w0 = k / (Na + k);
          float w1 = 1 / (2 * (Na + k));

          CMatrixF Pa(3, 3);
          CMatrixF L(3, 3);

          Pa.fill(0);
          Pa(0, 0) = Pa(1, 1) = (Na + k) * square(param.stdPixel);
          Pa(2, 2) = (Na + k) * square(param.stdDisp);

          // Cholesky decomposition
          Pa.chol(L);  // math::chol(Pa,L);

          vector<TPoint3D> B;  // B group
          TPoint3D meanB;      // Mean value of the B group
          CMatrixF Pb;         // Covariance of the B group

          B.resize(2 * Na + 1);  // Set of output values
          Pb.fill(0);            // Reset the output covariance

          CVectorFloat vAux, myPoint;  // Auxiliar vectors
          CVectorFloat meanA;          // Mean value of the A group

          vAux.resize(3);  // Set the variables size
          meanA.resize(3);
          myPoint.resize(3);

          // Mean input value: (c,r,d)
          meanA[0] = ptL.x;
          meanA[1] = ptL.y;
          meanA[2] = disp;

          // Output mean
          meanB.x = w0 * x3D;
          meanB.y = w0 * y3D;
          meanB.z = w0 * z3D;  // Add to the mean
          B[0].x = x3D;
          B[0].y = y3D;
          B[0].z = z3D;  // Insert into B

          for (i = 1; i <= 2 * Na; i++)
          {
            // Form the Ai value
            if (i <= Na)
            {
              vAux.asEigen() = L.col(i - 1);
              myPoint[0] = meanA[0] + vAux[0];
              myPoint[1] = meanA[1] + vAux[1];
              myPoint[2] = meanA[2] + vAux[2];
            }
            else
            {
              vAux = L.col((i - Na) - 1);
              myPoint[0] = meanA[0] - vAux[0];
              myPoint[1] = meanA[1] - vAux[1];
              myPoint[2] = meanA[2] - vAux[2];
            }

            // Pass the Ai through the functions:
            x3D = (myPoint[0] - param.K(0, 2)) * ((param.baseline)) / myPoint[2];
            y3D = (myPoint[1] - param.K(1, 2)) * ((param.baseline)) / myPoint[2];
            z3D = (param.K(0, 0)) * ((param.baseline)) / myPoint[2];

            // Add to the B mean computation and the B vector
            meanB.x = meanB.x + w1 * x3D;
            meanB.y = meanB.y + w1 * y3D;
            meanB.z = meanB.z + w1 * z3D;

            B[i].x = x3D;
            B[i].y = y3D;
            B[i].z = z3D;

          }  // end for 'i'

          // Output covariance
          for (i = 0; i <= 2 * Na; i++)
          {
            float weight = w1;
            CMatrixF v(3, 1);

            if (i == 0)  // The weight for the mean value of A is w0
              weight = w0;

            v(0, 0) = B[i].x - meanB.x;
            v(1, 0) = B[i].y - meanB.y;
            v(2, 0) = B[i].z - meanB.z;

            Pb.asEigen() += (weight * (v.asEigen() * v.transpose())).eval();
          }  // end for 'i'

          // Store it in the landmark
          lm.pose_cov_11 = Pb(0, 0);
          lm.pose_cov_12 = Pb(0, 1);
          lm.pose_cov_13 = Pb(0, 2);
          lm.pose_cov_22 = Pb(1, 1);
          lm.pose_cov_23 = Pb(1, 2);
          lm.pose_cov_33 = Pb(2, 2);
        }  // end case 'Prop_UT'
        break;

        case TStereoSystemParams::Prop_SUT:
        {
          // Parameters
          unsigned int Na = 3;
          unsigned int i;

          float a = param.factor_a;
          float b = param.factor_b;
          float k = param.factor_k;

          float lambda = square(a) * (Na + k) - Na;

          float w0_m = lambda / (Na + lambda);
          float w0_c = w0_m + (1 - square(a) + b);
          float w1 = 1 / (2 * (Na + lambda));

          CMatrixF Pa(3, 3);
          CMatrixF L(3, 3);

          Pa.fill(0);
          Pa(0, 0) = Pa(1, 1) = (Na + lambda) * square(param.stdPixel);
          Pa(2, 2) = (Na + lambda) * square(param.stdDisp);

          // Cholesky decomposition
          Pa.chol(L);  // math::chol(Pa,L);

          vector<TPoint3D> B;  // B group
          TPoint3D meanB;      // Mean value of the B group
          CMatrixF Pb;         // Covariance of the B group

          B.resize(2 * Na + 1);  // Set of output values
          Pb.fill(0);            // Reset the output covariance

          CVectorFloat vAux, myPoint;  // Auxiliar vectors
          CVectorFloat meanA;          // Mean value of the A group

          vAux.resize(3);  // Set the variables size
          meanA.resize(3);
          myPoint.resize(3);

          // Mean input value: (c,r,d)
          meanA[0] = ptL.x;
          meanA[1] = ptL.y;
          meanA[2] = disp;

          // Output mean
          meanB.x = w0_m * x3D;
          meanB.y = w0_m * y3D;
          meanB.z = w0_m * z3D;  // Add to the mean
          B[0].x = x3D;
          B[0].y = y3D;
          B[0].z = z3D;  // Insert into B

          for (i = 1; i <= 2 * Na; i++)
          {
            // Form the Ai value
            if (i <= Na)
            {
              vAux = L.row(i - 1);
              myPoint = meanA + vAux;
            }
            else
            {
              vAux = L.col((i - Na) - 1);
              myPoint = meanA - vAux;
            }

            // Pass the Ai through the functions:
            x3D = (myPoint[0] - param.K(0, 2)) * ((param.baseline)) / myPoint[2];
            y3D = (myPoint[1] - param.K(1, 2)) * ((param.baseline)) / myPoint[2];
            z3D = (param.K(0, 0)) * ((param.baseline)) / myPoint[2];

            // Add to the B mean computation and the B vector
            meanB.x = meanB.x + w1 * x3D;
            meanB.y = meanB.y + w1 * y3D;
            meanB.z = meanB.z + w1 * z3D;

            B[i].x = x3D;
            B[i].y = y3D;
            B[i].z = z3D;

          }  // end for 'i'

          // Output covariance
          for (i = 0; i <= 2 * Na; i++)
          {
            float weight = w1;
            CMatrixF v(3, 1);

            if (i == 0)  // The weight for the mean value of A is w0
              weight = w0_c;

            v(0, 0) = B[i].x - meanB.x;
            v(1, 0) = B[i].y - meanB.y;
            v(2, 0) = B[i].z - meanB.z;

            Pb.asEigen() += (weight * (v.asEigen() * v.transpose())).eval();
          }  // end for 'i'

          // Store it in the landmark
          lm.pose_cov_11 = Pb(0, 0);
          lm.pose_cov_12 = Pb(0, 1);
          lm.pose_cov_13 = Pb(0, 2);
          lm.pose_cov_22 = Pb(1, 1);
          lm.pose_cov_23 = Pb(1, 2);
          lm.pose_cov_33 = Pb(2, 2);
        }  // end case 'Prop_SUT'
        break;

      }  // end switch
      landmarks.landmarks.push_back(lm);
      itListL++;
      itListR++;
    }  // end else ( (z3D > param.minZ) && (z3D < param.maxZ) )
  }    // end for 'i'

  MRPT_END
}

/* -------------------------------------------------------
        StereoObs2RBObs #1
   ------------------------------------------------------- */
void vision::StereoObs2BRObs(
    const CMatchedFeatureList& inMatches,
    const CMatrixDouble33& intrinsicParams,
    double baseline,
    const CPose3D& sensorPose,
    const vector<double>& sg,
    CObservationBearingRange& outObs)
{
  // Compute the range and bearing
  double f = intrinsicParams(0, 0);   // Focal length in pixels
  double x0 = intrinsicParams(0, 2);  // Principal point column
  double y0 = intrinsicParams(1, 2);  // Principal point row
  double b = baseline;                // Stereo camera baseline
  double sg_c2 = square(sg[0]);       // Sigma of the column variable
  double sg_r2 = square(sg[1]);       // Sigma of the row variable
  double sg_d2 = square(sg[2]);       // Sigma of the disparity

  for (const auto& inMatche : inMatches)
  {
    double x = inMatche.first.keypoint.pt.x;  // Column of the feature
    double y = inMatche.first.keypoint.pt.y;  // Row of the feature

    double d = x - inMatche.second.keypoint.pt.x;  // Disparity
    double d2 = square(d);
    double k = square(b / d);

    // Projection equations according to a standard camera coordinate axis
    // (+Z forward & +Y downwards)
    // -------------------------------------------------------------------------------------------------------
    double X = (x - x0) * b / d;
    double Y = (y - y0) * b / d;
    double Z = f * b / d;

    // Projection equations according to a standard coordinate axis (+X
    // forward & +Z upwards)
    // -------------------------------------------------------------------------------------------------------
    // double X	= f * b / d;
    // double Y	= ( x0 - x ) * b / d;
    // double Z	= ( y0 - y ) * b / d;

    CObservationBearingRange::TMeasurement m;
    m.range = sqrt(square(X) + square(Y) + square(Z));
    m.yaw = atan2(Y, X);
    m.pitch = -asin(Z / m.range);
    m.landmarkID = inMatche.first.keypoint.ID;

    // Compute the covariance
    // Formula: S_BR = JG * (JF * diag(sg_c^2, sg_r^2, sg_d^2) * JF') * JG'
    //						\---------------------------------------/
    //											aux

    CMatrixDouble33 aux;

    // Jacobian equations according to a standard CAMERA coordinate axis (+Z
    // forward & +Y downwards)
    // -------------------------------------------------------------------------------------------------------
    aux(0, 0) = k * (sg_c2 + sg_d2 * square(x - x0) / d2);
    aux(0, 1) = aux(1, 0) = k * (sg_d2 * (x - x0) * (y - y0) / d2);
    aux(0, 2) = aux(2, 0) = k * (sg_d2 * (x - x0) * f / d2);

    aux(1, 1) = k * (sg_r2 + sg_d2 * square(y - y0) / d2);
    aux(1, 2) = aux(2, 1) = k * (sg_d2 * (y - y0) * f / d2);

    aux(2, 2) = k * (sg_d2 * square(f) / d2);

    // Jacobian equations according to a standard coordinate axis (+X
    // forward & +Z upwards)
    // -------------------------------------------------------------------------------------------------------
    CMatrixDouble33 JG;
    JG(0, 0) = X / m.range;
    JG(0, 1) = Y / m.range;
    JG(0, 2) = Z / m.range;

    JG(1, 0) = -Y / (square(X) + square(Y));
    JG(1, 1) = X / (square(X) + square(Y));
    JG(1, 2) = 0;

    JG(2, 0) = Z * X / (square(m.range) * sqrt(square(X) + square(Y)));
    JG(2, 1) = Z * Y / (square(m.range) * sqrt(square(X) + square(Y)));
    JG(2, 2) = -sqrt(square(X) + square(Y)) / square(m.range);

    m.covariance = mrpt::math::multiply_HCHt(JG, aux);

    outObs.sensedData.push_back(m);

  }  // end for

  // Indicate that the covariances have been calculated (for compatibility
  // with earlier versions)
  outObs.validCovariances = true;
  outObs.setSensorPose(sensorPose);
}  // end-StereoObs2BRObs

/* -------------------------------------------------------
        StereoObs2RBObs #2
   ------------------------------------------------------- */
void vision::StereoObs2BRObs(
    const CObservationStereoImages& inObs,
    const vector<double>& sg,
    CObservationBearingRange& outObs)
{
  // Local variables
  CFeatureExtraction fExt;
  CFeatureList leftList, rightList;
  CMatchedFeatureList matchList;
  unsigned int id = 0;

  // Extract features
  fExt.detectFeatures(inObs.imageLeft, leftList);
  fExt.detectFeatures(inObs.imageRight, rightList);

  // DEBUG:
  // CDisplayWindow		win1, win2;
  // win1.showImageAndPoints( inObs.imageLeft, leftList );
  // win2.showImageAndPoints( inObs.imageRight, rightList );

  // Match features
  // size_t nMatches =
  vision::matchFeatures(leftList, rightList, matchList);

  // Compute the range and bearing
  double f = inObs.leftCamera.fx();      // Focal length in pixels
  double x0 = inObs.leftCamera.cx();     // Principal point column
  double y0 = inObs.leftCamera.cy();     // Principal point row
  double b = inObs.rightCameraPose.x();  // Stereo camera baseline
  double sg_c2 = square(sg[0]);          // Sigma of the column variable
  double sg_r2 = square(sg[1]);          // Sigma of the row variable
  double sg_d2 = square(sg[2]);          // Sigma of the disparity

  for (auto itMatchList = matchList.begin(); itMatchList != matchList.end(); itMatchList++, id++)
  {
    double x = itMatchList->first.keypoint.pt.x;  // Column of the feature
    double y = itMatchList->first.keypoint.pt.y;  // Row of the feature

    double d = x - itMatchList->second.keypoint.pt.x;  // Disparity
    double d2 = square(d);
    double k = square(b / d);

    // Projection equations according to a standard camera coordinate axis
    // (+Z forward & +Y downwards)
    // -------------------------------------------------------------------------------------------------------
    double X = (x - x0) * b / d;
    double Y = (y - y0) * b / d;
    double Z = f * b / d;

    // Projection equations according to a standard coordinate axis (+X
    // forward & +Z upwards)
    // -------------------------------------------------------------------------------------------------------
    // double X	= f * b / d;
    // double Y	= ( x0 - x ) * b / d;
    // double Z	= ( y0 - y ) * b / d;

    CObservationBearingRange::TMeasurement m;
    m.range = sqrt(square(X) + square(Y) + square(Z));
    // m.yaw	= atan2( Y,X );
    // m.pitch = -asin( Z/m.range );
    m.yaw = atan2(X, Z);
    m.pitch = atan2(Y, Z);

    // Compute the covariance
    // Formula: S_BR = JG * (JF * diag(sg_c^2, sg_r^2, sg_d^2) * JF') * JG'
    //						\---------------------------------------/
    //											aux

    CMatrixDouble33 aux;

    // Jacobian equations according to a standard CAMERA coordinate axis (+Z
    // forward & +Y downwards)
    // -------------------------------------------------------------------------------------------------------
    aux(0, 0) = k * (sg_c2 + sg_d2 * square(x - x0) / d2);
    aux(0, 1) = aux(1, 0) = k * (sg_d2 * (x - x0) * (y - y0) / d2);
    aux(0, 2) = aux(2, 0) = k * (sg_d2 * (x - x0) * f / d2);

    aux(1, 1) = k * (sg_r2 + sg_d2 * square(y - y0) / d2);
    aux(1, 2) = aux(2, 1) = k * (sg_d2 * (y - y0) * f / d2);

    aux(2, 2) = k * (sg_d2 * square(f) / d2);

    // Jacobian equations according to a standard coordinate axis (+X
    // forward & +Z upwards)
    // -------------------------------------------------------------------------------------------------------
    CMatrixDouble33 JG;
    JG(0, 0) = X / m.range;
    JG(0, 1) = Y / m.range;
    JG(0, 2) = Z / m.range;

    JG(1, 0) = -Y / (square(X) + square(Y));
    JG(1, 1) = X / (square(X) + square(Y));
    JG(1, 2) = 0;

    JG(2, 0) = Z * X / (square(m.range) * sqrt(square(X) + square(Y)));
    JG(2, 1) = Z * Y / (square(m.range) * sqrt(square(X) + square(Y)));
    JG(2, 2) = -sqrt(square(X) + square(Y)) / square(m.range);

    // JF.multiply_HCHt( diag, aux );
    m.covariance = mrpt::math::multiply_HCHt(JG, aux);

    m.landmarkID = id;
    outObs.sensedData.push_back(m);
    outObs.fieldOfView_yaw = 2 * fabs(atan2(-x0, f));
    outObs.fieldOfView_pitch = 2 * fabs(atan2(-y0, f));

  }  // end for

  // Indicate that the covariances have been calculated (for compatibility
  // with earlier versions)
  outObs.validCovariances = true;
  outObs.setSensorPose(mrpt::poses::CPose3D(inObs.cameraPose));

}  // end StereoObs2BRObs

/* -------------------------------------------------------
        StereoObs2RBObs #3
   ------------------------------------------------------- */
void vision::StereoObs2BRObs(
    const CObservationVisualLandmarks& inObs, CObservationBearingRange& outObs)
{
  // For each of the 3D landmarks [X,Y,Z] we compute their range and bearing
  // representation.
  // The reference system is assumed to be that typical of cameras: +Z forward
  // and +X to the right.
  CLandmarksMap::TCustomSequenceLandmarks::const_iterator itCloud;
  for (itCloud = inObs.landmarks.landmarks.begin(); itCloud != inObs.landmarks.landmarks.end();
       ++itCloud)
  {
    CObservationBearingRange::TMeasurement m;
    m.range = sqrt(
        square(itCloud->pose_mean.x) + square(itCloud->pose_mean.y) + square(itCloud->pose_mean.z));

    // The reference system is assumed to be that typical robot operation:
    // +X forward and +Z upwards.
    m.yaw = atan2(itCloud->pose_mean.y, itCloud->pose_mean.x);
    m.pitch = -sin(itCloud->pose_mean.z / m.range);
    m.landmarkID = itCloud->ID;

    outObs.sensedData.push_back(m);
  }  // end for
}  // end StereoObs2BRObs

/* -------------------------------------------------------
        computeStereoRectificationMaps
   ------------------------------------------------------- */
void vision::computeStereoRectificationMaps(
    const TCamera& cam1,
    const TCamera& cam2,
    const poses::CPose3D& rightCameraPose,
    void* outMap1x,
    void* outMap1y,
    void* outMap2x,
    void* outMap2y)
{
  ASSERT_(cam1.ncols == cam2.ncols && cam1.nrows == cam2.nrows);

#if MRPT_HAS_OPENCV

  cv::Mat *mapx1, *mapy1, *mapx2, *mapy2;
  mapx1 = static_cast<cv::Mat*>(outMap1x);
  mapy1 = static_cast<cv::Mat*>(outMap1y);
  mapx2 = static_cast<cv::Mat*>(outMap2x);
  mapy2 = static_cast<cv::Mat*>(outMap2y);

  const int resX = cam1.ncols;
  const int resY = cam1.nrows;

  CMatrixDouble44 hMatrix;
  rightCameraPose.getHomogeneousMatrix(hMatrix);

  double rcTrans[3];
  rcTrans[0] = hMatrix(0, 3);
  rcTrans[1] = hMatrix(1, 3);
  rcTrans[2] = hMatrix(2, 3);

  double m1[3][3];
  for (unsigned int i = 0; i < 3; ++i)
    for (unsigned int j = 0; j < 3; ++j) m1[i][j] = hMatrix(i, j);

  double ipl[3][3], ipr[3][3], dpl[5], dpr[5];
  for (unsigned int i = 0; i < 3; ++i)
    for (unsigned int j = 0; j < 3; ++j)
    {
      ipl[i][j] = cam1.intrinsicParams(i, j);
      ipr[i][j] = cam2.intrinsicParams(i, j);
    }

  for (unsigned int i = 0; i < 5; ++i)
  {
    dpl[i] = cam1.dist[i];
    dpr[i] = cam2.dist[i];
  }

  cv::Mat R(3, 3, CV_64F, &m1);
  cv::Mat T(3, 1, CV_64F, &rcTrans);

  cv::Mat K1(3, 3, CV_64F, ipl);
  cv::Mat K2(3, 3, CV_64F, ipr);
  cv::Mat D1(1, 5, CV_64F, dpl);
  cv::Mat D2(1, 5, CV_64F, dpr);

  double _R1[3][3], _R2[3][3], _P1[3][4], _P2[3][4], _Q[4][4];
  cv::Mat R1(3, 3, CV_64F, _R1);
  cv::Mat R2(3, 3, CV_64F, _R2);
  cv::Mat P1(3, 4, CV_64F, _P1);
  cv::Mat P2(3, 4, CV_64F, _P2);
  cv::Mat Q(4, 4, CV_64F, _Q);

  cv::Size nSize(resX, resY);
  double alpha = 0.0;  // alpha value: 0.0 = zoom and crop the image so that
  // there's not black areas

  // OpenCV 2.3+ has this signature:
  cv::stereoRectify(
      K1, D1, K2, D2, nSize, R, T, R1, R2, P1, P2, Q, cv::CALIB_ZERO_DISPARITY, alpha);
  // Rest of arguments -> default

  cv::Size sz1, sz2;
  cv::initUndistortRectifyMap(K1, D1, R1, P1, cv::Size(resX, resY), CV_32FC1, *mapx1, *mapy1);
  cv::initUndistortRectifyMap(K2, D2, R2, P2, cv::Size(resX, resY), CV_32FC1, *mapx2, *mapy2);
/**/
#else
  THROW_EXCEPTION("MRPT has been compiled without OPENCV!");
#endif
}  // end computeStereoRectificationMaps

TStereoSystemParams::TStereoSystemParams()
{
  // K = defaultIntrinsicParamsMatrix(0, 640, 480);
  F.setZero();
  F(1, 2) = -1;
  F(2, 1) = 1;
}

/*-------------------------------------------------------------
      TStereoSystemParams: loadFromConfigFile
-------------------------------------------------------------*/
void TStereoSystemParams::loadFromConfigFile(const CConfigFileBase& iniFile, const string& section)
{
  int unc;
  unc = iniFile.read_int(section.c_str(), "uncPropagation", uncPropagation);
  switch (unc)
  {
    case 0:
      uncPropagation = Prop_Linear;
      break;
    case 1:
      uncPropagation = Prop_UT;
      break;
    case 2:
      uncPropagation = Prop_SUT;
      break;
  }  // end switch

  CVectorDouble k_vec(9);
  iniFile.read_vector(section.c_str(), "k_vec", CVectorDouble(), k_vec, false);
  for (unsigned int ii = 0; ii < 3; ++ii)
    for (unsigned int jj = 0; jj < 3; ++jj) K(ii, jj) = k_vec[ii * 3 + jj];

  CVectorDouble f_vec(9);
  iniFile.read_vector(section.c_str(), "f_vec", CVectorDouble(), f_vec, false);
  for (unsigned int ii = 0; ii < 3; ++ii)
    for (unsigned int jj = 0; jj < 3; ++jj) F(ii, jj) = f_vec[ii * 3 + jj];

  baseline = iniFile.read_float(section.c_str(), "baseline", baseline);
  stdPixel = iniFile.read_float(section.c_str(), "stdPixel", stdPixel);
  stdDisp = iniFile.read_float(section.c_str(), "stdDisp", stdDisp);
  maxZ = iniFile.read_float(section.c_str(), "maxZ", maxZ);
  minZ = iniFile.read_float(section.c_str(), "minZ", minZ);
  maxY = iniFile.read_float(section.c_str(), "maxY", maxY);
  factor_k = iniFile.read_float(section.c_str(), "factor_k", factor_k);
  factor_a = iniFile.read_float(section.c_str(), "factor_a", factor_a);
  factor_b = iniFile.read_float(section.c_str(), "factor_b", factor_b);
}  // end of loadFromConfigFile

/*---------------------------------------------------------------
          TStereoSystemParams: dumpToTextStream
  ---------------------------------------------------------------*/
void TStereoSystemParams::dumpToTextStream(std::ostream& out) const
{
  out << "\n----------- [vision::TStereoSystemParams] ------------ \n";
  out << "Method for 3D Uncert. \t= ";
  switch (uncPropagation)
  {
    case Prop_Linear:
      out << "Linear propagation\n";
      break;
    case Prop_UT:
      out << "Unscented Transform\n";
      break;
    case Prop_SUT:
      out << "Scaled Unscented Transform\n";
      break;
  }  // end switch

  out << mrpt::format("K\t\t\t= [%f\t%f\t%f]\n", K(0, 0), K(0, 1), K(0, 2));
  out << mrpt::format(" \t\t\t  [%f\t%f\t%f]\n", K(1, 0), K(1, 1), K(1, 2));
  out << mrpt::format(" \t\t\t  [%f\t%f\t%f]\n", K(2, 0), K(2, 1), K(2, 2));

  out << mrpt::format("F\t\t\t= [%f\t%f\t%f]\n", F(0, 0), F(0, 1), F(0, 2));
  out << mrpt::format(" \t\t\t  [%f\t%f\t%f]\n", F(1, 0), F(1, 1), F(1, 2));
  out << mrpt::format(" \t\t\t  [%f\t%f\t%f]\n", F(2, 0), F(2, 1), F(2, 2));

  out << mrpt::format("Baseline \t\t= %f\n", baseline);
  out << mrpt::format("Pixel std \t\t= %f\n", stdPixel);
  out << mrpt::format("Disparity std\t\t= %f\n", stdDisp);
  out << mrpt::format("Z maximum\t\t= %f\n", maxZ);
  out << mrpt::format("Z minimum\t\t= %f\n", minZ);
  out << mrpt::format("Y maximum\t\t= %f\n", maxY);

  out << mrpt::format("k Factor [UT]\t\t= %f\n", factor_k);
  out << mrpt::format("a Factor [UT]\t\t= %f\n", factor_a);
  out << mrpt::format("b Factor [UT]\t\t= %f\n", factor_b);
  out << "-------------------------------------------------------- \n";
}

void TMatchingOptions::loadFromConfigFile(const CConfigFileBase& iniFile, const string& section)
{
  int mm = iniFile.read_int(section.c_str(), "matching_method", matching_method);
  switch (mm)
  {
    case 0:
      matching_method = mmCorrelation;
      break;
    case 1:
      matching_method = mmDescriptorSIFT;
      break;
    case 2:
      matching_method = mmDescriptorSURF;
      break;
    case 3:
      matching_method = mmSAD;
      break;
    case 4:
      matching_method = mmDescriptorORB;
      break;
  }  // end switch

  useEpipolarRestriction =
      iniFile.read_bool(section.c_str(), "useEpipolarRestriction", useEpipolarRestriction);
  hasFundamentalMatrix =
      iniFile.read_bool(section.c_str(), "hasFundamentalMatrix", hasFundamentalMatrix);
  parallelOpticalAxis =
      iniFile.read_bool(section.c_str(), "parallelOpticalAxis", parallelOpticalAxis);
  useXRestriction = iniFile.read_bool(section.c_str(), "useXRestriction", useXRestriction);
  addMatches = iniFile.read_bool(section.c_str(), "addMatches", addMatches);
  useDisparityLimits = iniFile.read_bool(section.c_str(), "useDisparityLimits", useDisparityLimits);

  min_disp = iniFile.read_float(section.c_str(), "min_disp", min_disp);
  max_disp = iniFile.read_float(section.c_str(), "max_disp", max_disp);

  epipolar_TH = iniFile.read_float(section.c_str(), "epipolar_TH", epipolar_TH);
  maxEDD_TH = iniFile.read_float(section.c_str(), "maxEDD_TH", maxEDD_TH);
  EDD_RATIO = iniFile.read_float(section.c_str(), "minDIF_TH", EDD_RATIO);
  minCC_TH = iniFile.read_float(section.c_str(), "minCC_TH", minCC_TH);
  minDCC_TH = iniFile.read_float(section.c_str(), "minDCC_TH", minDCC_TH);
  rCC_TH = iniFile.read_float(section.c_str(), "rCC_TH", rCC_TH);
  maxEDSD_TH = iniFile.read_float(section.c_str(), "maxEDSD_TH", maxEDSD_TH);
  EDSD_RATIO = iniFile.read_float(section.c_str(), "EDSD_RATIO", EDSD_RATIO);
  maxSAD_TH = iniFile.read_float(section.c_str(), "maxSAD_TH", maxSAD_TH);
  SAD_RATIO = iniFile.read_float(section.c_str(), "SAD_RATIO", SAD_RATIO);
  maxORB_dist = iniFile.read_float(section.c_str(), "maxORB_dist", maxORB_dist);

  estimateDepth = iniFile.read_bool(section.c_str(), "estimateDepth", estimateDepth);
  maxDepthThreshold = iniFile.read_float(section.c_str(), "maxDepthThreshold", maxDepthThreshold);
  //	fx                  = iniFile.read_float(section.c_str(),"fx",fx);
  //	cx                  = iniFile.read_float(section.c_str(),"cx",cx);
  //	cy                  = iniFile.read_float(section.c_str(),"cy",cy);
  //	baseline            =
  // iniFile.read_float(section.c_str(),"baseline",baseline);
}  // end TMatchingOptions::loadFromConfigFile

/*---------------------------------------------------------------
          TMatchingOptions: dumpToTextStream
  ---------------------------------------------------------------*/
void TMatchingOptions::dumpToTextStream(std::ostream& out) const
{
  out << "\n----------- [vision::TMatchingOptions] ------------ \n";
  out << "Matching method:                ";
  switch (matching_method)
  {
    case mmCorrelation:
      out << "Cross Correlation\n";
      out << mrpt::format("· Min. CC. Threshold:           %f\n", minCC_TH);
      out << mrpt::format("· Min. Dif. CC Threshold:       %f\n", minDCC_TH);
      out << mrpt::format("· Max. Ratio CC Threshold:      %f\n", rCC_TH);
      break;
    case mmDescriptorSIFT:
      out << "SIFT descriptor\n";
      out << mrpt::format("· Max. EDD Threshold:           %f\n", maxEDD_TH);
      out << mrpt::format("· EDD Ratio:                    %f\n", EDD_RATIO);
      break;
    case mmDescriptorSURF:
      out << "SURF descriptor\n";
      out << mrpt::format("· EDD Ratio:                    %f\n", maxEDSD_TH);
      out << mrpt::format("· Min. CC Threshold:            %f\n", EDSD_RATIO);
      break;
    case mmSAD:
      out << "SAD\n";
      out << mrpt::format("· Max. Dif. SAD Threshold:      %f\n", maxSAD_TH);
      out << mrpt::format("· Ratio SAD Threshold:          %f\n", SAD_RATIO);
      break;
    case mmDescriptorORB:
      out << "ORB\n";
      out << mrpt::format("· Max. distance between desc:	%f\n", maxORB_dist);
      break;
  }  // end switch
  out << mrpt::format("Epipolar Thres:                 %.2f px\n", epipolar_TH);
  out << "Using epipolar restriction?:    " << (useEpipolarRestriction ? "Yes\n" : "No\n");
  out << "Has Fundamental Matrix?:        " << (hasFundamentalMatrix ? "Yes\n" : "No\n");
  out << "Are camera axis parallel?:      " << (parallelOpticalAxis ? "Yes\n" : "No\n");
  out << "Use X-coord restriction?:       " << (useXRestriction ? "Yes\n" : "No\n");
  out << "Use disparity limits?:       " << (useDisparityLimits ? "Yes\n" : "No\n");
  if (useDisparityLimits)
    out << mrpt::format("· Min/max disp limits:          %.2f/%.2f px\n", min_disp, max_disp);
  out << "Estimate depth?:                " << (estimateDepth ? "Yes\n" : "No\n");
  if (estimateDepth)
  {
    out << mrpt::format("· Maximum depth allowed:        %f m\n", maxDepthThreshold);
  }
  out << "Add matches to list?:           ";
  out << (addMatches ? "Yes\n" : "No\n");
  out << "-------------------------------------------------------- \n";
}  // end TMatchingOptions::dumpToTextStream
